#include <cmath>

#include "graph_data.h"

namespace
{
  double const PI =3.14159265358979323846;
  double const EARTH_RADIUS = 6371000.;
  
  inline double deg_to_rad(double deg)
  {
    return deg * PI / 180.;
  }
} // namespace

double orthodromic_distance(vertex_data_t const & v1, vertex_data_t const & v2)
{   
  
  double lat1 = deg_to_rad(v1.lat);
  double lon1 = deg_to_rad(v1.lon);  
  double lat2 = deg_to_rad(v2.lat);
  double lon2 = deg_to_rad(v2.lon);
  
  double cos_lat1 = std::cos(lat1);
  double cos_lat2 = std::cos(lat2);
  double sin_lat1 = std::sin(lat1);
  double sin_lat2 = std::sin(lat2);

  double dx = cos_lat2 * std::cos(lon2) - cos_lat1 * std::cos(lon1);
  double dy = cos_lat2 * std::sin(lon2) - cos_lat1 * std::sin(lon1);
  double dz = sin_lat2 - sin_lat1;
  
  double ch = std::sqrt(dx * dx + dy * dy + dz * dz);
  
  
  return EARTH_RADIUS * 2. * std::asin(ch / 2.); 
}